
#include <asm.h>

.global C_SYMBOL(RECT888To666)

C_SYMBOL(RECT888To666):
//.long 0xe7fddefe

    STMFD	SP!, {R4-R12,LR}
Start:

    LDR R11, [R0, #0x0]                 //pSrc
    LDR	R8,  [R0, #0xC]                 //SrcStep
    LDR	R10, [R1, #0x0]                 //pDst
	LDR	R12, [R1, #0xC]                 //DstStep


    LDR	R4, [R0, #0x4]                  //width
	LDR	R7, [R0, #0x8]                  //height

	MOV LR, R4, LSL #2                  // LR = src pitch

    MOV     R5, #0

    LDR     R4, = 0x00000002
    TMCR    wCGR1, R4
    LDR     R4, = 0x00000004
    TMCR    wCGR2, R4
    LDR     R4, = 0x00000006
    TMCR    wCGR3, R4
    LDR     R4, = 0x000000FC
    TMCRR   wR13, R4 , R4
    LDR     R4, = 0x0000FC00
    TMCRR   wR14, R4 , R4
    LDR     R4, = 0x00FC0000
    TMCRR   wR15, R4 , R4
    WZERO   wR1



LoopLine:
	MOV	R3, LR                          // R3 = src pitch
	MOV	R2, R10
	MOV	R0, R11
	B Alpha_8u_C4ARow

Next:
	SUB	R7, R7,	#1
	ADD	R11, R11, R8
	ADD	R10, R10, R12
	CMP	R7, #0
	BHI	LoopLine

End:

	LDMFD	SP!, {R4-R12,PC}


Alpha_8u_C4ARow:

    ADD	R6, R2,	#7
    BIC	R6, R6,	#7
    SUB	R6, R6,	R2
    CMP	R6, #0
    BEQ	_Aligned8_

_Process_One_:

    WLDRW   wR4, [R0], #4
    CMP	R6, R3

    MOVGT	R6, R3
    SUB	R3, R3,	R6

_Loop_One_:

    SUBS	R6, R6,	#4

    WAND    wR0, wR4, wR13          //B
    WSRLWG  wR0, wR0, wCGR1
    WAND    wR1, wR4, wR14          //G
    WSRLWG  wR1, wR1, wCGR2
    WAND    wR2, wR4, wR15          //R
    WSRLWG  wR2, wR2, wCGR3
    WLDRWGT wR4, [R0], #4

    WOR     wR0, wR0, wR1
    WOR     wR0, wR0, wR2

    WSTRW   wR0, [R2], #4

    BGT	_Loop_One_

    CMP	R3, #0
    BEQ     Next

_Aligned8_:
    SUBS	R3, R3,	#0x10
    ADDMI	R6, R3,	#0x10
    ADDMI	R3, R3,	#0x10
    BMI	_Process_One_
    AND	R4, R0,	#7
    CMP R4, #0
    BNE	_UnAligned8_

    WLDRD   wR3, [R0], #8
    WLDRD   wR7, [R0], #8

_Loop_Aligned8_:

    SUBS	R3, R3,	#0x10
    PLD	[R0,#0x20]

    WAND    wR0, wR3, wR13          //B
    WSRLWG  wR0, wR0, wCGR1
    WAND    wR1, wR3, wR14          //G
    WSRLWG  wR1, wR1, wCGR2
    WAND    wR2, wR3, wR15          //R
    WSRLWG  wR2, wR2, wCGR3
    WLDRDGE wR3, [R0], #8
    WOR     wR0, wR0, wR1
    WOR     wR0, wR0, wR2

    WAND    wR4, wR7, wR13          //B
    WSRLWG  wR4, wR4, wCGR1
    WAND    wR5, wR7, wR14          //G
    WSRLWG  wR5, wR5, wCGR2
    WAND    wR6, wR7, wR15          //R
    WSRLWG  wR6, wR6, wCGR3
    WLDRDGE wR7, [R0], #8
    WOR     wR4, wR4, wR5
    WOR     wR4, wR4, wR6

    WSTRD   wR0, [R2], #8
    WSTRD   wR4, [R2], #8

    BGE	_Loop_Aligned8_
    ADDS	R3, R3,	#0x10
    BEQ     Next
    MOVGT	R6, R3
    //SUBGT   R2, R2, #8
    BGT	_Process_One_


_UnAligned8_:
//.long 0xe7fddefe
    BIC	R0, R0,	#7
    TMCR    wCGR0, R4

    WLDRD   wR3, [R0], #8
    WLDRD   wR7, [R0], #8

_Loop_UnAligned8_:
@.long 0xe7fddefe
    TST	R4, #7
    WLDRDNE wR12, [R0]

    SUBS	R3, R3,	#0x10
    PLD	[R0,#0x20]

    WALIGNR0 wR3 ,wR3, wR7

    WAND    wR0, wR3, wR13          //B
    WSRLWG  wR0, wR0, wCGR1
    WAND    wR1, wR3, wR14          //G
    WSRLWG  wR1, wR1, wCGR2
    WAND    wR2, wR3, wR15          //R
    WSRLWG  wR2, wR2, wCGR3
    WLDRDGE wR3, [R0], #8
    WOR     wR0, wR0, wR1
    WOR     wR0, wR0, wR2

    WALIGNR0 wR7 ,wR7, wR12

    WAND    wR4, wR7, wR13          //B
    WSRLWG  wR4, wR4, wCGR1
    WAND    wR5, wR7, wR14          //G
    WSRLWG  wR5, wR5, wCGR2
    WAND    wR6, wR7, wR15          //R
    WSRLWG  wR6, wR6, wCGR3
    WLDRDGE wR7, [R0], #8
    WOR     wR4, wR4, wR5
    WOR     wR4, wR4, wR6

    WSTRD   wR0, [R2], #8
    WSTRD   wR4, [R2], #8

    BGE	_Loop_UnAligned8_

    ADD	R0, R0,	R4
    ADDS	R3, R3,	#0x10
    BEQ     Next
    MOVGT	R6, R3
    BGT	_Process_One_

